<!-- saved from url=(0014)about:internet -->
<html>
<head>
<meta http-equiv="content-type" content="text/html; charset=utf-8" />
<title>RTW Report - nav_h_ekf.c</title>
<script language="JavaScript" type="text/javascript" src="rtwreport_utils.js"></script>
</head>
<body bgcolor="#eeeeee" text="#1122aa" onload="try {if (top) { if (top.rtwFileOnLoad) top.rtwFileOnLoad(window.document); else local_onload();}} catch(err) {};">
<p>
<table border="0" cellspacing="0" cellpadding="6" width="100%%" height="100%%"><tr><td width="100%%" valign="top" bgcolor="#ffffff">
<h4>File: <a href="../nav_h_ekf.c" target="rtwreport_document_frame" id="linkToText_plain">nav_h_ekf.c</a></h4>
<pre id="RTWcode">
<a name="1"><font color="#888888 "><i>    1   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="2"><font color="#888888 "><i>    2   </i></font></a><font color="#117755"><I> * File: nav_h_ekf.c</i></font>
<a name="3"><font color="#888888 "><i>    3   </i></font></a><font color="#117755"><I> *</i></font>
<a name="4"><font color="#888888 "><i>    4   </i></font></a><font color="#117755"><I> * MATLAB Coder version            : 2.6</i></font>
<a name="5"><font color="#888888 "><i>    5   </i></font></a><font color="#117755"><I> * C/C++ source code generated on  : 03-Jul-2017 03:24:26</i></font>
<a name="6"><font color="#888888 "><i>    6   </i></font></a><font color="#117755"><I> */</i></font>
<a name="7"><font color="#888888 "><i>    7   </i></font></a>
<a name="8"><font color="#888888 "><i>    8   </i></font></a><font color="#117755"><i>/* Include files */</i></font>
<a name="9"><font color="#888888 "><i>    9   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;rt_nonfinite.h&quot;</font>
<a name="10"><font color="#888888 "><i>   10   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;nav_h_ekf.h&quot;</font>
<a name="11"><font color="#888888 "><i>   11   </i></font></a>
<a name="12"><font color="#888888 "><i>   12   </i></font></a><font color="#117755"><i>/* Variable Definitions */</i></font>
<a name="13"><font color="#888888 "><i>   13   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_X">X</a>[4];
<a name="14"><font color="#888888 "><i>   14   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_P">P</a>[16];
<a name="15"><font color="#888888 "><i>   15   </i></font></a>
<a name="16"><font color="#888888 "><i>   16   </i></font></a><font color="#117755"><i>/* Function Definitions */</i></font>
<a name="17"><font color="#888888 "><i>   17   </i></font></a>
<a name="18"><font color="#888888 "><i>   18   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="19"><font color="#888888 "><i>   19   </i></font></a><font color="#117755"><I> * 作者 ytf</i></font>
<a name="20"><font color="#888888 "><i>   20   </i></font></a><font color="#117755"><I> * 日期 2017年4月27日</i></font>
<a name="21"><font color="#888888 "><i>   21   </i></font></a><font color="#117755"><I> * 状态转移协方差矩阵</i></font>
<a name="22"><font color="#888888 "><i>   22   </i></font></a><font color="#117755"><I> * H 预测矩阵</i></font>
<a name="23"><font color="#888888 "><i>   23   </i></font></a><font color="#117755"><I> * R 观测噪声方差</i></font>
<a name="24"><font color="#888888 "><i>   24   </i></font></a><font color="#117755"><I> * F 状态转移矩阵</i></font>
<a name="25"><font color="#888888 "><i>   25   </i></font></a><font color="#117755"><I> * I</i></font>
<a name="26"><font color="#888888 "><i>   26   </i></font></a><font color="#117755"><I> * Z 测量矩阵</i></font>
<a name="27"><font color="#888888 "><i>   27   </i></font></a><font color="#117755"><I> * Arguments    : const float Q[16]</i></font>
<a name="28"><font color="#888888 "><i>   28   </i></font></a><font color="#117755"><I> *                const float H[8]</i></font>
<a name="29"><font color="#888888 "><i>   29   </i></font></a><font color="#117755"><I> *                const float R[4]</i></font>
<a name="30"><font color="#888888 "><i>   30   </i></font></a><font color="#117755"><I> *                const float F[16]</i></font>
<a name="31"><font color="#888888 "><i>   31   </i></font></a><font color="#117755"><I> *                const float I[16]</i></font>
<a name="32"><font color="#888888 "><i>   32   </i></font></a><font color="#117755"><I> *                const float Z[2]</i></font>
<a name="33"><font color="#888888 "><i>   33   </i></font></a><font color="#117755"><I> *                unsigned char up</i></font>
<a name="34"><font color="#888888 "><i>   34   </i></font></a><font color="#117755"><I> *                float *h</i></font>
<a name="35"><font color="#888888 "><i>   35   </i></font></a><font color="#117755"><I> *                float *v</i></font>
<a name="36"><font color="#888888 "><i>   36   </i></font></a><font color="#117755"><I> *                float *a</i></font>
<a name="37"><font color="#888888 "><i>   37   </i></font></a><font color="#117755"><I> *                float *a_a</i></font>
<a name="38"><font color="#888888 "><i>   38   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="39"><font color="#888888 "><i>   39   </i></font></a><font color="#117755"><I> */</i></font>
<a name="40"><font color="#888888 "><i>   40   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_nav_h_ekf">nav_h_ekf</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> Q[16], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> H[8], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> R[4], <font color="#112266"><b>const</b></font>
<a name="41"><font color="#888888 "><i>   41   </i></font></a>               <font color="#112266"><b>float</b></font> F[16], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> I[16], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> Z[2], <font color="#112266"><b>unsigned</b></font> <font color="#112266"><b>char</b></font>
<a name="42"><font color="#888888 "><i>   42   </i></font></a>               up, <font color="#112266"><b>float</b></font> *h, <font color="#112266"><b>float</b></font> *v, <font color="#112266"><b>float</b></font> *a, <font color="#112266"><b>float</b></font> *a_a)
<a name="43"><font color="#888888 "><i>   43   </i></font></a><b>{</b>
<a name="44"><font color="#888888 "><i>   44   </i></font></a>  <font color="#112266"><b>float</b></font> K[4];
<a name="45"><font color="#888888 "><i>   45   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="46"><font color="#888888 "><i>   46   </i></font></a>  <font color="#112266"><b>float</b></font> b_F[16];
<a name="47"><font color="#888888 "><i>   47   </i></font></a>  <font color="#112266"><b>int</b></font> r2;
<a name="48"><font color="#888888 "><i>   48   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="49"><font color="#888888 "><i>   49   </i></font></a>  <font color="#112266"><b>float</b></font> a21;
<a name="50"><font color="#888888 "><i>   50   </i></font></a>  <font color="#112266"><b>float</b></font> y[8];
<a name="51"><font color="#888888 "><i>   51   </i></font></a>  <font color="#112266"><b>float</b></font> b_K[8];
<a name="52"><font color="#888888 "><i>   52   </i></font></a>  <font color="#112266"><b>float</b></font> a22;
<a name="53"><font color="#888888 "><i>   53   </i></font></a>  <font color="#112266"><b>float</b></font> b_Z[2];
<a name="54"><font color="#888888 "><i>   54   </i></font></a>  <font color="#112266"><b>float</b></font> B[16];
<a name="55"><font color="#888888 "><i>   55   </i></font></a>
<a name="56"><font color="#888888 "><i>   56   </i></font></a>  <font color="#117755"><i>/*  状态变量 */</i></font>
<a name="57"><font color="#888888 "><i>   57   </i></font></a>  <font color="#117755"><i>/*  状态协方差矩阵 */</i></font>
<a name="58"><font color="#888888 "><i>   58   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="59"><font color="#888888 "><i>   59   </i></font></a>    K[i] = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i];
<a name="60"><font color="#888888 "><i>   60   </i></font></a>  <b>}</b>
<a name="61"><font color="#888888 "><i>   61   </i></font></a>
<a name="62"><font color="#888888 "><i>   62   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="63"><font color="#888888 "><i>   63   </i></font></a>    <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] = 0.0F;
<a name="64"><font color="#888888 "><i>   64   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="65"><font color="#888888 "><i>   65   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] += F[i + (r2 &lt;&lt; 2)] * K[r2];
<a name="66"><font color="#888888 "><i>   66   </i></font></a>    <b>}</b>
<a name="67"><font color="#888888 "><i>   67   </i></font></a>
<a name="68"><font color="#888888 "><i>   68   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="69"><font color="#888888 "><i>   69   </i></font></a>      b_F[i + (r2 &lt;&lt; 2)] = 0.0F;
<a name="70"><font color="#888888 "><i>   70   </i></font></a>      <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="71"><font color="#888888 "><i>   71   </i></font></a>        b_F[i + (r2 &lt;&lt; 2)] += F[i + (k &lt;&lt; 2)] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[k + (r2 &lt;&lt; 2)];
<a name="72"><font color="#888888 "><i>   72   </i></font></a>      <b>}</b>
<a name="73"><font color="#888888 "><i>   73   </i></font></a>    <b>}</b>
<a name="74"><font color="#888888 "><i>   74   </i></font></a>  <b>}</b>
<a name="75"><font color="#888888 "><i>   75   </i></font></a>
<a name="76"><font color="#888888 "><i>   76   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="77"><font color="#888888 "><i>   77   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="78"><font color="#888888 "><i>   78   </i></font></a>      a21 = 0.0F;
<a name="79"><font color="#888888 "><i>   79   </i></font></a>      <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="80"><font color="#888888 "><i>   80   </i></font></a>        a21 += b_F[i + (k &lt;&lt; 2)] * F[r2 + (k &lt;&lt; 2)];
<a name="81"><font color="#888888 "><i>   81   </i></font></a>      <b>}</b>
<a name="82"><font color="#888888 "><i>   82   </i></font></a>
<a name="83"><font color="#888888 "><i>   83   </i></font></a>      <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] = a21 + Q[i + (r2 &lt;&lt; 2)];
<a name="84"><font color="#888888 "><i>   84   </i></font></a>    <b>}</b>
<a name="85"><font color="#888888 "><i>   85   </i></font></a>  <b>}</b>
<a name="86"><font color="#888888 "><i>   86   </i></font></a>
<a name="87"><font color="#888888 "><i>   87   </i></font></a>  <font color="#112266"><b>if</b></font> (up == 1) <b>{</b>
<a name="88"><font color="#888888 "><i>   88   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="89"><font color="#888888 "><i>   89   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 2; r2++) <b>{</b>
<a name="90"><font color="#888888 "><i>   90   </i></font></a>        y[i + (r2 &lt;&lt; 2)] = 0.0F;
<a name="91"><font color="#888888 "><i>   91   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="92"><font color="#888888 "><i>   92   </i></font></a>          y[i + (r2 &lt;&lt; 2)] += <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (k &lt;&lt; 2)] * H[r2 + (k &lt;&lt; 1)];
<a name="93"><font color="#888888 "><i>   93   </i></font></a>        <b>}</b>
<a name="94"><font color="#888888 "><i>   94   </i></font></a>      <b>}</b>
<a name="95"><font color="#888888 "><i>   95   </i></font></a>    <b>}</b>
<a name="96"><font color="#888888 "><i>   96   </i></font></a>
<a name="97"><font color="#888888 "><i>   97   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="98"><font color="#888888 "><i>   98   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="99"><font color="#888888 "><i>   99   </i></font></a>        b_K[i + (r2 &lt;&lt; 1)] = 0.0F;
<a name="100"><font color="#888888 "><i>  100   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="101"><font color="#888888 "><i>  101   </i></font></a>          b_K[i + (r2 &lt;&lt; 1)] += H[i + (k &lt;&lt; 1)] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[k + (r2 &lt;&lt; 2)];
<a name="102"><font color="#888888 "><i>  102   </i></font></a>        <b>}</b>
<a name="103"><font color="#888888 "><i>  103   </i></font></a>      <b>}</b>
<a name="104"><font color="#888888 "><i>  104   </i></font></a>    <b>}</b>
<a name="105"><font color="#888888 "><i>  105   </i></font></a>
<a name="106"><font color="#888888 "><i>  106   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="107"><font color="#888888 "><i>  107   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 2; r2++) <b>{</b>
<a name="108"><font color="#888888 "><i>  108   </i></font></a>        a21 = 0.0F;
<a name="109"><font color="#888888 "><i>  109   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="110"><font color="#888888 "><i>  110   </i></font></a>          a21 += b_K[i + (k &lt;&lt; 1)] * H[r2 + (k &lt;&lt; 1)];
<a name="111"><font color="#888888 "><i>  111   </i></font></a>        <b>}</b>
<a name="112"><font color="#888888 "><i>  112   </i></font></a>
<a name="113"><font color="#888888 "><i>  113   </i></font></a>        K[i + (r2 &lt;&lt; 1)] = a21 + R[i + (r2 &lt;&lt; 1)];
<a name="114"><font color="#888888 "><i>  114   </i></font></a>      <b>}</b>
<a name="115"><font color="#888888 "><i>  115   </i></font></a>    <b>}</b>
<a name="116"><font color="#888888 "><i>  116   </i></font></a>
<a name="117"><font color="#888888 "><i>  117   </i></font></a>    <font color="#112266"><b>if</b></font> ((<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(K[1]) &gt; (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(K[0])) <b>{</b>
<a name="118"><font color="#888888 "><i>  118   </i></font></a>      i = 1;
<a name="119"><font color="#888888 "><i>  119   </i></font></a>      r2 = 0;
<a name="120"><font color="#888888 "><i>  120   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="121"><font color="#888888 "><i>  121   </i></font></a>      i = 0;
<a name="122"><font color="#888888 "><i>  122   </i></font></a>      r2 = 1;
<a name="123"><font color="#888888 "><i>  123   </i></font></a>    <b>}</b>
<a name="124"><font color="#888888 "><i>  124   </i></font></a>
<a name="125"><font color="#888888 "><i>  125   </i></font></a>    a21 = K[r2] / K[i];
<a name="126"><font color="#888888 "><i>  126   </i></font></a>    a22 = K[2 + r2] - a21 * K[2 + i];
<a name="127"><font color="#888888 "><i>  127   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="128"><font color="#888888 "><i>  128   </i></font></a>      b_K[k + (i &lt;&lt; 2)] = y[k] / K[i];
<a name="129"><font color="#888888 "><i>  129   </i></font></a>      b_K[k + (r2 &lt;&lt; 2)] = (y[4 + k] - b_K[k + (i &lt;&lt; 2)] * K[2 + i]) / a22;
<a name="130"><font color="#888888 "><i>  130   </i></font></a>      b_K[k + (i &lt;&lt; 2)] -= b_K[k + (r2 &lt;&lt; 2)] * a21;
<a name="131"><font color="#888888 "><i>  131   </i></font></a>    <b>}</b>
<a name="132"><font color="#888888 "><i>  132   </i></font></a>
<a name="133"><font color="#888888 "><i>  133   </i></font></a>    <font color="#117755"><i>/* 增益 */</i></font>
<a name="134"><font color="#888888 "><i>  134   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="135"><font color="#888888 "><i>  135   </i></font></a>      a21 = 0.0F;
<a name="136"><font color="#888888 "><i>  136   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="137"><font color="#888888 "><i>  137   </i></font></a>        a21 += H[i + (r2 &lt;&lt; 1)] * <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[r2];
<a name="138"><font color="#888888 "><i>  138   </i></font></a>      <b>}</b>
<a name="139"><font color="#888888 "><i>  139   </i></font></a>
<a name="140"><font color="#888888 "><i>  140   </i></font></a>      b_Z[i] = Z[i] - a21;
<a name="141"><font color="#888888 "><i>  141   </i></font></a>    <b>}</b>
<a name="142"><font color="#888888 "><i>  142   </i></font></a>
<a name="143"><font color="#888888 "><i>  143   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="144"><font color="#888888 "><i>  144   </i></font></a>      a21 = 0.0F;
<a name="145"><font color="#888888 "><i>  145   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 2; r2++) <b>{</b>
<a name="146"><font color="#888888 "><i>  146   </i></font></a>        a21 += b_K[i + (r2 &lt;&lt; 2)] * b_Z[r2];
<a name="147"><font color="#888888 "><i>  147   </i></font></a>      <b>}</b>
<a name="148"><font color="#888888 "><i>  148   </i></font></a>
<a name="149"><font color="#888888 "><i>  149   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] += a21;
<a name="150"><font color="#888888 "><i>  150   </i></font></a>    <b>}</b>
<a name="151"><font color="#888888 "><i>  151   </i></font></a>
<a name="152"><font color="#888888 "><i>  152   </i></font></a>    memcpy(&amp;B[0], &amp;<a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[0], <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>) &lt;&lt; 4);
<a name="153"><font color="#888888 "><i>  153   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="154"><font color="#888888 "><i>  154   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="155"><font color="#888888 "><i>  155   </i></font></a>        a21 = 0.0F;
<a name="156"><font color="#888888 "><i>  156   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="157"><font color="#888888 "><i>  157   </i></font></a>          a21 += b_K[i + (k &lt;&lt; 2)] * H[k + (r2 &lt;&lt; 1)];
<a name="158"><font color="#888888 "><i>  158   </i></font></a>        <b>}</b>
<a name="159"><font color="#888888 "><i>  159   </i></font></a>
<a name="160"><font color="#888888 "><i>  160   </i></font></a>        b_F[i + (r2 &lt;&lt; 2)] = I[i + (r2 &lt;&lt; 2)] - a21;
<a name="161"><font color="#888888 "><i>  161   </i></font></a>      <b>}</b>
<a name="162"><font color="#888888 "><i>  162   </i></font></a>    <b>}</b>
<a name="163"><font color="#888888 "><i>  163   </i></font></a>
<a name="164"><font color="#888888 "><i>  164   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="165"><font color="#888888 "><i>  165   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="166"><font color="#888888 "><i>  166   </i></font></a>        <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] = 0.0F;
<a name="167"><font color="#888888 "><i>  167   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="168"><font color="#888888 "><i>  168   </i></font></a>          <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] += b_F[i + (k &lt;&lt; 2)] * B[k + (r2 &lt;&lt; 2)];
<a name="169"><font color="#888888 "><i>  169   </i></font></a>        <b>}</b>
<a name="170"><font color="#888888 "><i>  170   </i></font></a>      <b>}</b>
<a name="171"><font color="#888888 "><i>  171   </i></font></a>    <b>}</b>
<a name="172"><font color="#888888 "><i>  172   </i></font></a>  <b>}</b>
<a name="173"><font color="#888888 "><i>  173   </i></font></a>
<a name="174"><font color="#888888 "><i>  174   </i></font></a>  <font color="#112266"><b>if</b></font> (up == 0) <b>{</b>
<a name="175"><font color="#888888 "><i>  175   </i></font></a>    a21 = 0.0F;
<a name="176"><font color="#888888 "><i>  176   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="177"><font color="#888888 "><i>  177   </i></font></a>      K[i] = 0.0F;
<a name="178"><font color="#888888 "><i>  178   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="179"><font color="#888888 "><i>  179   </i></font></a>        K[i] += H[1 + (r2 &lt;&lt; 1)] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[r2 + (i &lt;&lt; 2)];
<a name="180"><font color="#888888 "><i>  180   </i></font></a>      <b>}</b>
<a name="181"><font color="#888888 "><i>  181   </i></font></a>
<a name="182"><font color="#888888 "><i>  182   </i></font></a>      a21 += K[i] * H[1 + (i &lt;&lt; 1)];
<a name="183"><font color="#888888 "><i>  183   </i></font></a>    <b>}</b>
<a name="184"><font color="#888888 "><i>  184   </i></font></a>
<a name="185"><font color="#888888 "><i>  185   </i></font></a>    a22 = a21 + R[3];
<a name="186"><font color="#888888 "><i>  186   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="187"><font color="#888888 "><i>  187   </i></font></a>      a21 = 0.0F;
<a name="188"><font color="#888888 "><i>  188   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="189"><font color="#888888 "><i>  189   </i></font></a>        a21 += <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] * H[1 + (r2 &lt;&lt; 1)];
<a name="190"><font color="#888888 "><i>  190   </i></font></a>      <b>}</b>
<a name="191"><font color="#888888 "><i>  191   </i></font></a>
<a name="192"><font color="#888888 "><i>  192   </i></font></a>      K[i] = a21 / a22;
<a name="193"><font color="#888888 "><i>  193   </i></font></a>    <b>}</b>
<a name="194"><font color="#888888 "><i>  194   </i></font></a>
<a name="195"><font color="#888888 "><i>  195   </i></font></a>    <font color="#117755"><i>/* 增益 */</i></font>
<a name="196"><font color="#888888 "><i>  196   </i></font></a>    a21 = 0.0F;
<a name="197"><font color="#888888 "><i>  197   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="198"><font color="#888888 "><i>  198   </i></font></a>      a21 += H[1 + (i &lt;&lt; 1)] * <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i];
<a name="199"><font color="#888888 "><i>  199   </i></font></a>    <b>}</b>
<a name="200"><font color="#888888 "><i>  200   </i></font></a>
<a name="201"><font color="#888888 "><i>  201   </i></font></a>    a21 = Z[1] - a21;
<a name="202"><font color="#888888 "><i>  202   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="203"><font color="#888888 "><i>  203   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] += K[i] * a21;
<a name="204"><font color="#888888 "><i>  204   </i></font></a>    <b>}</b>
<a name="205"><font color="#888888 "><i>  205   </i></font></a>
<a name="206"><font color="#888888 "><i>  206   </i></font></a>    memcpy(&amp;B[0], &amp;<a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[0], <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>) &lt;&lt; 4);
<a name="207"><font color="#888888 "><i>  207   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="208"><font color="#888888 "><i>  208   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="209"><font color="#888888 "><i>  209   </i></font></a>        b_F[i + (r2 &lt;&lt; 2)] = I[i + (r2 &lt;&lt; 2)] - K[i] * H[1 + (r2 &lt;&lt; 1)];
<a name="210"><font color="#888888 "><i>  210   </i></font></a>      <b>}</b>
<a name="211"><font color="#888888 "><i>  211   </i></font></a>    <b>}</b>
<a name="212"><font color="#888888 "><i>  212   </i></font></a>
<a name="213"><font color="#888888 "><i>  213   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="214"><font color="#888888 "><i>  214   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 4; r2++) <b>{</b>
<a name="215"><font color="#888888 "><i>  215   </i></font></a>        <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] = 0.0F;
<a name="216"><font color="#888888 "><i>  216   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 4; k++) <b>{</b>
<a name="217"><font color="#888888 "><i>  217   </i></font></a>          <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + (r2 &lt;&lt; 2)] += b_F[i + (k &lt;&lt; 2)] * B[k + (r2 &lt;&lt; 2)];
<a name="218"><font color="#888888 "><i>  218   </i></font></a>        <b>}</b>
<a name="219"><font color="#888888 "><i>  219   </i></font></a>      <b>}</b>
<a name="220"><font color="#888888 "><i>  220   </i></font></a>    <b>}</b>
<a name="221"><font color="#888888 "><i>  221   </i></font></a>  <b>}</b>
<a name="222"><font color="#888888 "><i>  222   </i></font></a>
<a name="223"><font color="#888888 "><i>  223   </i></font></a>  *h = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[0];
<a name="224"><font color="#888888 "><i>  224   </i></font></a>  *v = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[1];
<a name="225"><font color="#888888 "><i>  225   </i></font></a>  *a = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[2];
<a name="226"><font color="#888888 "><i>  226   </i></font></a>  *a_a = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[3];
<a name="227"><font color="#888888 "><i>  227   </i></font></a><b>}</b>
<a name="228"><font color="#888888 "><i>  228   </i></font></a>
<a name="229"><font color="#888888 "><i>  229   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="230"><font color="#888888 "><i>  230   </i></font></a><font color="#117755"><I> * Arguments    : void</i></font>
<a name="231"><font color="#888888 "><i>  231   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="232"><font color="#888888 "><i>  232   </i></font></a><font color="#117755"><I> */</i></font>
<a name="233"><font color="#888888 "><i>  233   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_nav_h_ekf_init">nav_h_ekf_init</a>(<font color="#112266"><b>void</b></font>)
<a name="234"><font color="#888888 "><i>  234   </i></font></a><b>{</b>
<a name="235"><font color="#888888 "><i>  235   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> fv0[16] = <b>{</b> 10.0F, 0.0F, 0.0F, 0.0F, 0.0F, 10.0F, 0.0F,
<a name="236"><font color="#888888 "><i>  236   </i></font></a>    0.0F, 0.0F, 0.0F, 10.0F, 0.0F, 0.0F, 0.0F, 0.0F, 100.0F <b>}</b>;
<a name="237"><font color="#888888 "><i>  237   </i></font></a>
<a name="238"><font color="#888888 "><i>  238   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="239"><font color="#888888 "><i>  239   </i></font></a>  memcpy(&amp;<a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[0], &amp;fv0[0], <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>) &lt;&lt; 4);
<a name="240"><font color="#888888 "><i>  240   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="241"><font color="#888888 "><i>  241   </i></font></a>    <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] = 0.0F;
<a name="242"><font color="#888888 "><i>  242   </i></font></a>  <b>}</b>
<a name="243"><font color="#888888 "><i>  243   </i></font></a><b>}</b>
<a name="244"><font color="#888888 "><i>  244   </i></font></a>
<a name="245"><font color="#888888 "><i>  245   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="246"><font color="#888888 "><i>  246   </i></font></a><font color="#117755"><I> * File trailer for nav_h_ekf.c</i></font>
<a name="247"><font color="#888888 "><i>  247   </i></font></a><font color="#117755"><I> *</i></font>
<a name="248"><font color="#888888 "><i>  248   </i></font></a><font color="#117755"><I> * [EOF]</i></font>
<a name="249"><font color="#888888 "><i>  249   </i></font></a><font color="#117755"><I> */</i></font>
<a name="250"><font color="#888888 "><i>  250   </i></font></a></pre>
</td></tr></table>
</p>
</body>
</html>